#include "control.h"
bool UAV_Arming = false;
int16_t dif_PPM_Databuf[10] = {0,0,0,0,0,0,0,0,0,0};
//////////////////姿态环//////////////////
PID_TYPE PID_ROL={1,0,0,  0,0,0,0,10000};//角度外环参数
PID_TYPE PID_PIT={1,0,0,  0,0,0,0,10000};
PID_TYPE PID_YAW={2,0,0,  0,0,0,0,200}; 

PID_TYPE PID_ROL_R={0.65,0.9,0,  0,0,0,0,10000};//角速度内环参数 3.5
PID_TYPE PID_PIT_R={0.65,0.9,0,  0,0,0,0,10000};
PID_TYPE PID_YAW_R={6,0,0,   0,0,0,0,10};

//////////////////高度环//////////////////
ADRC_Data ADRC_HIGH = {1.5,0,0.2,     0.01,500, 50,300,800,}; 
ADRC_Data ADRC_HIGH_R ={0.9,0.02,0, 0.01,1000, 50,400,1100,}; 

//////////////////位置环//////////////////
PID_TYPE PID_POSIT_X={1,0,0,  0,0,0,0,400};//X轴位置环 1  1.2   1
PID_TYPE PID_POSIT_Y={1,0,0,  0,0,0,0,400}; //Y轴位置环

PID_TYPE PID_POSITION_X={8,0,0,  0,0,0,0,30};//X轴位置环  5 1 飘动  4   0.24,0.015   
PID_TYPE PID_POSITION_Y={8,0,0,  0,0,0,0,30}; //Y轴位置环  晃动  微分会加大颤动，抑制晃动

PID_TYPE PID_POSITION_ax={0.3,0,0,  0,0,0,0,400};
PID_TYPE PID_POSITION_ay={0.3,0,0,  0,0,0,0,400};



ADRC_Data ADRC_HIGH_ = {1, 1, 0,     0.01,500, 50,300,800,}; //高度环

PID_TYPE PID_HIGH={1.5,0,0,  0,0,0,0,4000};//高度外环参数 1.8  单环：0.9,0.8
PID_TYPE PID_HIGH_R={0.5,0,0.012,};//速度内环参数  0.8

//////////////////ADRC控制参数///////////////////
ADRC_Data ADRC_ROL = {3,0,0,     0.01,500, 50,300,800,}; 
ADRC_Data ADRC_PIT = {3,0,0,     0.01,500, 50,300,800,};

ADRC_Data ADRC_ROL_R={1,0,0, 0.01,1000, 50,400,1100,}; 
ADRC_Data ADRC_PIT_R={1,0,0, 0.01,1000, 50,400,1100,};



float Moto_1=0.0f,Moto_2=0.0f,Moto_3=0.0f,Moto_4=0.0f;
float speedbase=0,highaim=0,highaim_1,yaw=0,yaw_aim=0,yaw_1,rol=0,rol_1=0,pit=0,pit_1=0,V_X_Aim,V_Y_Aim;  
float Rol_Aim,Pit_Aim,Yaw_Aim;
float PWM_BASE=1500,highaim_min=5,highaim_max=160;//基础油门，初始目标高度
int Uav_Enable=0,Auto_Mode=0;
 
float Test_light,Test_right,Test_Yaw,Test_yaw;
float x1[2],x2[2],x3[2];
uint8_t Control_Time=0;
void Control(void) 
{
	if(Uav_Enable == 1)
	{
		speedbase=(PPM_Databuf[2])+8;//总拉力
		if(UAV_Arming == true)
		{

			if(PPM_Databuf[2]<1050)
				Moto_Pwm(1050,1050,1050,1050);//怠速
			else
			{
				speedbase=PPM_Databuf[2]+4;//总拉力
				if(speedbase>PWM_BASE) speedbase=PWM_BASE; //speedbase限制在1000以内
			
				//从遥控获得期望姿态，数值范围-200~200
				//俯仰
				Pit_Aim=(PPM_Databuf[1]-1500)/10;
				if(fabs(Pit_Aim)<3) Pit_Aim=0; 
				if(Pit_Aim>50) Pit_Aim=50; 
				if(Pit_Aim<-50) Pit_Aim=-50;		
				//横滚
				Rol_Aim=(PPM_Databuf[0]-1500)/10;
				if(fabs(Rol_Aim)<3) Rol_Aim=0; 
				if(Rol_Aim>50) Rol_Aim=50; 
				if(Rol_Aim<-50) Rol_Aim=-50;				
				//偏航
				Yaw_Aim=(PPM_Databuf[3]-1500)/2.78;
				if(fabs(Yaw_Aim)<5) Yaw_Aim=0; 
				if(Yaw_Aim>180) Yaw_Aim=180; 
				if(Yaw_Aim<-180) Yaw_Aim=-180;												
			
				////////////////姿态串级控制/////////////////
				
				PID_Pit(&PID_PIT,-Pit_Aim,Att_Angle.pit+5);//角度外环 遥控器输出作为姿态外环输入
				PID_Rol(&PID_ROL,Rol_Aim,Att_Angle.rol-1.5f); 
				
				PID_Angle_R(&PID_ROL_R,PID_ROL.OutPut,Gyr_rad.Y*RadtoDeg);//角速度内环 
				PID_Angle_R(&PID_PIT_R,PID_PIT.OutPut,Gyr_rad.X*RadtoDeg);
						
				/////////////////偏航控制//////////////
				if(Yaw_Aim==0) //遥控器无打杆时，串级控制
				{
					PID_Yaw(&PID_YAW,yaw_aim,Att_Angle.yaw);
					PID_Yaw_R(&PID_YAW_R,PID_YAW.OutPut,Gyr_rad.Z*RadtoDeg+2); 
				}
				else //遥控器打杆时，打杆信号给角速度环，内环控制
				{
					PID_YAW_C(&PID_YAW_R,Yaw_Aim,Gyr_rad.Z*RadtoDeg); 
					yaw_aim=Att_Angle.yaw;
				}
					

				
				//////////////////动力分配/////////////////
				if(Uav_Enable&&speedbase>30&&ms10>300) 
				{
					Moto_1 =  speedbase /*+ PID_PIT_R.OutPut - PID_ROL_R.OutPut   ;*/- PID_YAW_R.OutPut;
					Moto_2 =  speedbase /*- PID_PIT_R.OutPut + PID_ROL_R.OutPut   ;*/- PID_YAW_R.OutPut;
					Moto_3 =  speedbase /*- PID_PIT_R.OutPut - PID_ROL_R.OutPut   ;*/+ PID_YAW_R.OutPut;
					Moto_4 =  speedbase /*+ PID_PIT_R.OutPut + PID_ROL_R.OutPut   ;*/+ PID_YAW_R.OutPut;
				}
				Moto_Pwm(Moto_1,Moto_2,Moto_3,Moto_4);
				
			}
		}
		else
		{
			Moto_Pwm(1000,1000,1000,1000);//怠速
		}
	}
	//UAV_Enable = 0
	else
	{
		Moto_Pwm(1000,1000,1000,1000);
	}
	

//	else {Moto_1=0;Moto_2=0;Moto_3=0;Moto_4=0;}
//	Safety_Check();//飞机失控检测
	for(uint8_t i=0;i<10;i++)
		Last_PPM_Databuf[i] = PPM_Databuf[i];
}
/******************************************************************************************
*函  数：void Safety_Check(void)
*功  能：飞机姿态安全监测
*参  数：无
*返回值：无
*备  注：如果飞机角度和加速度异常就将飞机上锁并停止电机，防止电机狂转打坏桨叶
*******************************************************************************************/
void Safety_Check(void)
{
  if(fabs(Att_Angle.pit)>50.0f||fabs(Att_Angle.rol)>50.0f)
	{
		Moto_1=1000;
		Moto_2=1000;
		Moto_3=1000;
		Moto_4=1000;
	}
}
